#include "stm32f10x.h" 
#include "Delay.h"
#include "OLED.h"
#include "MPU6050.h"
#include "Motor.h"
#include "time.h"
#include "Encoder.h"
#include "Serial.h"

int Speed;
float S1,S2,Roll,Pitch,Yaw;
int16_t AccX,AccY,AccZ,GyroX,GyroY,GyroZ,Accel_Angle_x,Accel_Angle_y;
int32_t S;

int main(void)
{
	OLED_Init();
	MPU6050_Init();
	Timer_Init();
	Encoder_Init();
	Motor_Init();	
	Serial_Init();

	while (1)
	{
		Get_Angle(&AccX,&AccY,&AccZ,&GyroX,&GyroY,&GyroZ,&Accel_Angle_x,&Accel_Angle_y);
		S = Balance(Pitch)-Velocity(S1,S2);
		Motor_SetSpeed1(S); 
		Motor_SetSpeed2(S); 
	}
}


void TIM4_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
	{
		S1= Speed_Get1();
		S2 = Speed_Get2();
		TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
	}
}
